Machines such as, for example, dozers, motor graders, wheel loaders, wheel tractor scrapers, and other types of heavy equipment are used to perform a variety of tasks at a worksite. Autonomously and semi-autonomously controlled machines are capable of operating with little or no human input by relying on information received from various machine systems. For example, based on machine movement input, terrain input, and/or machine operational input, a machine can be controlled to remotely and/or automatically complete a programmed task. By receiving appropriate feedback from each of the different machine systems during performance of the task, continuous adjustments to machine operation can be made that help to ensure precision and safety in completion of the task. In order to do so, however, the information provided by the different machine systems should be accurate and reliable. The velocity and angular orientation (for example, yaw, pitch, and roll) of the machine are a few of such parameters, for which accuracy may be important for control of the machine and its operation.
Conventional machines typically utilize a navigation or positioning system to determine these parameters. Some conventional machines utilize a combination of one or more of Global Navigation Satellite System (GNSS) data, a Distance Measurement Indicator (DMI) or odometer measurement data, Inertial Measurement Unit (IMU) data, etc. to determine these parameters. Some machines utilize perception systems including 3D scanning devices to determine these parameters. The 3D scanning devices may be one or more of 3D LIDARs (light detection and ranging), flash LIDARS, and 3D cameras.
Machines that use 3D LIDARs scan their surrounding environment to obtain a series of 3D point clouds. The 3D LIDAR unit in such machines may include a plurality of light sources, such as lasers. Each laser may generate a laser beam which is directed at various points of a worksite. The LIDAR unit may further include one or more detector devices that receive the laser beams after reflection off of various points of worksite. Based on the time between generating the laser beam and receiving the reflected laser beam (referred to as time-of-flight measurements), the LIDAR unit may determine a distance to the corresponding point. In such a manner, the LIDAR unit may generate a 3D point cloud image representative of a part of worksite. Each data point in this 3D point cloud image may include a distance from the LIDAR unit to a detected point of worksite. Once a series of such 3D point clouds are obtained, different machine parameters such as yaw, roll, pitch, etc. may be obtained by aligning the different point clouds with one another.
Methods that align the different point clouds to determine displacement and orientation of the machine are typically referred to in the literature as registration methods. Assume that we have two point clouds (a first and second point cloud) of the same object. The only difference between the two point clouds is that one of them (the second point cloud) is transformed, either: rotated (rotated around the axis) or translated (moved along the axis), or both, with respect to the original/first point cloud. The goal or solution of a registration method is to be able to move the second point cloud in such a way as to negate the change (either the rotation or translation) it has undergone and bring it back to where the original/first point cloud is positioned. The amount of rotation or translation required to align the second point cloud with the first point cloud may provide the angular orientation and lateral displacement of the machine.
An exemplary point cloud registration method that may be used to align two point clouds is disclosed in “Fully Automatic Registration of 3D Point Clouds” by Makadia et al. The registration technique disclosed in the Makadia publication generates an Extended Gaussian Image (EGI) for each of the point clouds and aligns the point clouds using correlation between the EGIs.
Although the registration technique disclosed in the Makadia publication may be useful in aligning 3D point clouds, the disclosed registration technique has some drawbacks. For example, the registration technique of the Makadia publication may not be able to determine lateral displacement between the point clouds and hence, may not provide the ability to determine velocity of the machine. Further, the disclosed technique may be highly reliant on object characteristics and hence, may not be applicable when the detected objects are uniform in all directions, such as a ball.
The motion estimation system of the present disclosure is directed toward solving one or more of the problems set forth above and/or other problems of the prior art.